import matplotlib.pyplot as plt
import numpy as np
import math

delta = 0.1
T = 10
k = list(range(1, int(T / delta) + 1))
x_measure = [0.0] * len(k)
t_0 = np.arange(0, T, 0.01)
XK = [0] * len(t_0)
for i in range(0, len(t_0)):
    XK[i] = 3 + t_0[i]
t = [0] * len(k)
K1 = [0] * len(k)
K2 = [0] * len(k)
#RES = [0.0] * len(k)
wk = [0] * len(k)
x_prev = 0.0
xd_prev = 0.0
x_hat = [0] * len(k)
xd_hat = [0] * len(k)
e1 = [0] * len(k)
e2 = [0] * len(k)

for i in range(0, len(k)):
    t[i] = (k[i] - 1) * delta
    wk[i] = np.random.normal(loc= 0.0, scale= 5.0,size= None)
    x_measure[i] = t[i] + 3 + wk[i]
for i in range(0, len(k)):
    K1[i] = (2 *(2*k[i] - 1)) / (k[i] * (k[i] + 1))
    K2[i] = 6 / (k[i] * (k[i] + 1) * delta)
    RES = x_measure[i] - x_prev - (xd_prev * delta)
    x_hat[i] = x_prev + xd_prev * delta + K1[i] * RES
    xd_hat[i] = xd_prev + K2[i] * RES
    x_prev = x_hat[i]
    xd_prev = xd_hat[i]
    e1[i] = x_hat[i] - (t[i] + 3)

plt.xlabel('Time(Sec)')
plt.ylabel('xhat')
plt.plot(t, x_measure, '-o',label = 'measures')
plt.plot(t, x_hat, label = 'first-order R least squares filters')
plt.plot(t_0, XK, '+' ,label = 'True')
plt.legend()
plt.show()

plt.xlabel("Time(Sec)")
plt.ylabel("error between estimates ang true signal")
plt.plot(t,e1,label = 'error between estimates ang true signal')
plt.grid()
plt.legend()
plt.show()

plt.xlabel("Time(Sec)")
plt.ylabel("x Dot")
plt.axhline(y = 1,c = 'k',lw = 2,label = 'the derivative of true signal')
plt.plot(t,xd_hat,'--',label = 'the derivative of estimates')
plt.ylim(-5,5)
plt.legend()
plt.show()